//Simon Mikulcik Fall 2011 rewritten from C++ to incorporate a camera feed

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.camera.*;
import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends SimpleRobot {

	RobotDrive myRobot;
	Jaguar mainArmController;
	Victor secondaryArmController;
	Jaguar deployController;
	//AnalogChannel mainEnc;
	//Jaguar secArmController;
	Joystick rightJoy;
	Joystick leftJoy;
	// driver station object for getting selections
	DriverStation ds;
	DriverStationLCD dsLCD;
	// digital inputs for line tracking sensors	
	DigitalInput leftLineFollow;			
	DigitalInput middleLineFollow;
	DigitalInput rightLineFollow;
	Solenoid lineTracker;
	// digital inputs for limit switches
	DigitalInput limitMainTravelPosition;
	DigitalInput limitSecondaryTravelPosition;
	DigitalInput limitMinibot;
	//armTimer
	Timer timer;
	Solenoid gripOpenSolenoid;
	Solenoid gripCloseSolenoid;
	//compressor
	Compressor compress;
	
	//tee shirt cannon
	Relay cannonSolenoid;
	
	//gyro
	Gyro gyro;
	
	//camera 
	//AxisCamera cam;
	//HSLImage img;
		
	
	//button variables
	static final int trigger = 1;	
	static final int low = 4;	
	static final int middle = 3;	
	static final int high = 5;	
	static final int load = 2;	
	static final int clutch = 7;
	static final int grip = 6;	
	static final int deploy = 8;
	static final int deployControl = 9;
	
	int i;
	//joystick variables
	int buttonNum;
	boolean joyOutput;
	int lastButton;
	boolean buttonPressed;

	//arm varibles
	boolean isDefaultArmPosition;
	//gripper
	boolean gripState;
		
	
	public RobotTemplate()
	{
		myRobot = new RobotDrive(1, 2);
		mainArmController = new Jaguar(4,3);
		secondaryArmController = new Victor(4,4);
		deployController = new Jaguar(4,5);
		// = new AnalogChannel(1,1);
		//secArmController = new Jaguar(4,2);
		rightJoy = new Joystick(1);
		leftJoy = new Joystick(2);
		// driver station instance for digital I/O
		ds = DriverStation.getInstance();
		dsLCD = DriverStationLCD.getInstance();
		//ds.DriverStation();
		// digitial inputs for line following sensors.  Channel number may need
		// to be changed depending on physical robot setup
		/*leftLineFollow = new DigitalInput(4, 1);
		middleLineFollow = new DigitalInput(4, 2);
		rightLineFollow = new DigitalInput(4, 3);*/
		//compressor
		compress = new Compressor(4, 7, 4, 1);
		//invert each of the motor outputs
		//myRobot.setInvertedMotor(RobotDrive.kFrontRightMotor, true);
		//myRobot.setInvertedMotor(RobotDrive.kFrontLeftMotor, true);
		//myRobot.setInvertedMotor(RobotDrive.kRearLeftMotor, true);
		// watchdog can be left alone for 0.1 s before starving
		myRobot.setExpiration(0.1);
		lineTracker = new Solenoid(7, 5);
		// digital inputs for limit switches for controlling where the main arm
		// rotates to stop
		limitMainTravelPosition = new DigitalInput(4, 4);
		limitSecondaryTravelPosition = new DigitalInput(4, 5);
		limitMinibot = new DigitalInput(4, 6);
		
		//arm timers
		timer = new Timer();
		timer.start();
		
		//solenoids
		lineTracker.set(true);
		
		gripOpenSolenoid = new Solenoid(7, 1);
		gripCloseSolenoid = new Solenoid(7, 2);

		//GRIPPER INIT
		gripState = true;	//is gripper closed
		gripOpenSolenoid.set(true);
		Timer.delay(0.030);
		gripOpenSolenoid.set(false);
		
		// COMPRESSOR INIT
		compress.start();
		
		//tee shirt cannon
		cannonSolenoid = new Relay(4, 4, Relay.Direction.kForward);
		cannonSolenoid.set(Relay.Value.kOff);
		//gyro
		gyro = new Gyro(1);
		gyro.reset();

		AxisCamera cam = AxisCamera.getInstance();
		cam.writeResolution(AxisCamera.ResolutionT.k320x240);
		//AxisCameraStart();
	}// end Constructor()
		
/*
	binary value to track the lines
*/
public void Autonomous() {
	/*
	double defaultSteeringGain = 0.65;	// default value for steering gain

	int binaryValue;
	int previousValue = 0;
	double steeringGain;

	// the power profiles for the straight and forked robot path. They are
	// different to let the robot drive more slowly as the robot approaches
	// the fork on the forked line case.
	double forkProfile[] = {0.70, 0.70, 0.70, 0.70, 0.60, 0.60, 0.60, 0.60, 0.55, 0.55, 0.50, 0.50, 0.40, 0.40, 0.0};
	double straightProfile[] = {0.70, 0.70, 0.70, 0.70, 0.60, 0.60, 0.60, 0.60, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.0};

	double *powerProfile;  // the selected power profile
	
	// set the straight vs forked path variables as read from the DS digital
	// inputs or the I/O setup panel on the driver station.
	boolean straightLine = ds.getDigitalIn(1);
	powerProfile = (straightLine) ? straightProfile : forkProfile;
	double stopTime = (straightLine) 
	? 2.0 : 4.0;
	boolean goLeft = !ds.getDigitalIn(2) && !straightLine;
	printf("StraightLine: %d\n", straightLine);
	printf("GoingLeft: %d\n", goLeft);

	bool atCross = false;	// true when robot has reached end

	// set up timer for 8 second max driving time and use the timer to
	// pick values from the power profile arrays
	timer.Reset();
	
	int oldTimeInSeconds = -1;
	double time;
	double speed, turn;

	TopArmPosition();//move arm up before getting to the location

			
	// loop until either we hit the "T" at the end or 8 seconds has
	// elapsed. The time to the end should be less than 7 seconds
	// for either path.
	while ((time = timer.get()) < 15.0 && !atCross) {
		int timeInSeconds = (int) time;
		int leftValue = leftLineFollow.get() ? 1 : 0;	// read the line tracking sensors
		int middleValue = middleLineFollow.get() ? 1 : 0;
		int rightValue = rightLineFollow.get() ? 1 : 0;

	    // compute the single value from the 3 sensors. Notice that the bits
	    // for the outside sensors are flipped depending on left or right
	    // fork. Also the sign of the steering direction is different for left/right.
		if (goLeft) {
			binaryValue = leftValue * 4 + middleValue * 2 + rightValue;
			steeringGain = -defaultSteeringGain;
		} else {
			binaryValue = rightValue * 4 + middleValue * 2 + leftValue;
			steeringGain = defaultSteeringGain;
		}

		speed = powerProfile[timeInSeconds];	// speed value for this time
		turn = 0;								// default to no turn
//
 // binary
 // 000 0
 // 001 1
 // 010 2
 // 011 3
 // 100 4
 // 101 5
 // 110 6
 // 111 7
 //
		switch (binaryValue) {
			case 1:					// just the outside sensor - drive straight
				turn = 0;
				break;
			case 7:					// all sensors - maybe at the "T"
			if (time> stopTime) {
				atCross = true;
				speed = 0;
			}
			break;
			case 0:					// no sensors - apply previous correction
			if (previousValue == 0 || previousValue == 1) {
				turn = steeringGain;
			}
			else {
				turn = -steeringGain;
			}
			break;
			default:				// anything else, steer back to the line
			turn = -steeringGain;
		}
		// useful debugging output for tuning your power profile and steering gain
		if(binaryValue != previousValue)
			printf("Time: %2.2f sensor: %d speed: %1.2f turn: %1.2f atCross: %d\n", time, binaryValue, speed, turn, atCross);
		// move the robot forward
		myRobot.arcadeDrive(speed, turn);
		if (binaryValue != 0) {previousValue = binaryValue;}
		oldTimeInSeconds = timeInSeconds;
		Timer.delay(0.01);
	}*/
	/*
	DefaultArmPosition();
	timer.Reset();
	while(timer.get()<1.5)
	{
		myRobot.arcadeDrive(-1,0);
		Timer.delay(0.005);
	}
	myRobot.arcadeDrive(0.0 ,0.0);
	TopArmPosition();//move arm up before getting to the location
	//GripRelease();
	Timer.delay(1);
	timer.Reset();
	while(timer.get()<0.25)
	{
		myRobot.arcadeDrive(0.5,0);
		Timer.delay(0.005);
	}
	myRobot.arcadeDrive(0.0,0.0);
	Timer.delay(0.5);
	
	DefaultArmPosition();
	// stop driving when finished
	myRobot.arcadeDrive(0.0, 0.0);*/
}//end autonomous
double angle;
/*
	test joystick controls with the arm
*/public void operatorControl()
{
	//myRobot.setSafetyEnabled(true);

	//AxisCamera cam = AxisCamera.getInstance();
	//cam.getImage();
	gyro.reset();
	while (isOperatorControl())
	{
		dsLCD.updateLCD();
		//img = cam.getImage();
		readJoystick();
		angle = gyro.getAngle();
		dsLCD.println(DriverStationLCD.Line.kUser2,1,"Gyro readout: "+ angle);
		// drive with arcade style (use left stick)
		myRobot.arcadeDrive(leftJoy); 
		
		if(!rightJoy.getRawButton(deployControl)){
		mainArmController.set(rightJoy.getX(Joystick.Hand.kLeft));			//speed for moving the arm
		//StempYvalue = tempYvalue * tempYvalue * tempYvalue;
		secondaryArmController.set(rightJoy.getY(Joystick.Hand.kRight));			//speed for moving the arm
		}
		
		Timer.delay(0.005);				// Timer.delay for a motor update time
	} // end while
}// end OperatorControl() 


/**
	read joystick buttons
*/
public void readJoystick()
{
	buttonPressed = false;
if(rightJoy.getRawButton(low) && rightJoy.getRawButton(trigger)){
	if(lastButton != 1){lastButton = 1;buttonPressed = true; LowArmPosition();}
	}
/*//CPP CODE!!!!!!!
//if(limitMainTravelPosition){
	//if(lastButton != 10){lastButton = 10; LowArmPosition();}
	//}
if(rightJoy . getRawButton(middle) && rightJoy . getRawButton(trigger)){
	if(lastButton != 2){lastButton = 2;buttonPressed = true; MiddleArmPosition();}
}
if(rightJoy . getRawButton(high) && rightJoy . getRawButton(trigger)){
	if(lastButton != 3){lastButton = 3;buttonPressed = true; TopArmPosition();}
}
if(rightJoy . getRawButton(load) && rightJoy . getRawButton(trigger)){
	if(lastButton != 4){lastButton = 4;buttonPressed = true; LoadArmPosition();}
}
if(rightJoy . getRawButton(defaultArm) && rightJoy . getRawButton(trigger)){
	if(lastButton != 6){lastButton = 6;buttonPressed = true; DefaultArmPosition();}
}
if(rightJoy . getRawButton(trigger)){
	if(lastButton != 5){lastButton = 5;buttonPressed = true; GripRelease();}
}
//if(rightJoy . getRawButton(deploy) && rightJoy . getRawButton(trigger)){
	//if(lastButton != 7){lastButton = 7; Deploy();}
//}*/


	if(rightJoy.getRawButton(grip) && lastButton != grip){
		GripRelease();
		lastButton = grip;
	}
	if(!rightJoy.getRawButton(grip)){lastButton = 0;}
while(leftJoy.getRawButton(clutch)){
		myRobot.drive(0,0);
		mainArmController.set(0);
		secondaryArmController.set(0);
		if(leftJoy.getRawButton(clutch) && leftJoy.getRawButton(trigger) && lastButton != trigger){
			//open solenoid
			lastButton = trigger;
			cannonSolenoid.set(Relay.Value.kOn);
			Timer.delay(0.01);
			cannonSolenoid.set(Relay.Value.kOff);
		}
		if(!leftJoy.getRawButton(trigger))lastButton = 0;
}	

while(rightJoy.getRawButton(clutch)){
	myRobot.drive(0,0);
	mainArmController.set(0);
	if(rightJoy.getRawButton(clutch) && rightJoy.getRawButton(trigger) && lastButton != trigger){
	secondaryArmController.set(0);
		//open solenoid
		lastButton = trigger;
		cannonSolenoid.set(Relay.Value.kOn);
		Timer.delay(0.01);
		cannonSolenoid.set(Relay.Value.kOff);
	}
	if(!rightJoy.getRawButton(trigger))lastButton = 0;
}
if(buttonPressed = false){lastButton = 0;}	//if no button was pressed set to zero
if(rightJoy.getRawButton(10)){gripOpenSolenoid.set(true);lastButton = 0;dsLCD.println(DriverStationLCD.Line.kUser2,1,"Grip Close!");}
if(!rightJoy.getRawButton(10)){gripOpenSolenoid.set(false);}
if(rightJoy.getRawButton(11)){gripCloseSolenoid.set(true);dsLCD.println(DriverStationLCD.Line.kUser2,1,"Grip Open!");}
if(!rightJoy.getRawButton(11)){gripCloseSolenoid.set(false);}

//CPP CODE!!!!!
/*if(rightJoy.getRawButton(deployControl)){deployController.set(rightJoy.getX(Joystick.Hand.kRight));}
if(!rightJoy.getRawButton(deployControl)){deployController.set(0);}*/
}//end readJoystick

public void LowArmPosition()
{
	if(isDefaultArmPosition){DefaultArmPosition();}
	isDefaultArmPosition = false;
	dsLCD.println(DriverStationLCD.Line.kUser2,1,"Entering LowArmPosition");
	if(gyro.getAngle() < 68.0){	
		while(rightJoy.getRawButton(1) && gyro.getAngle() < 63.0){
			mainArmController.set(1);
			secondaryArmController.set(rightJoy.getY(Joystick.Hand.kRight));
			Timer.delay(0.005);
		}
	}else{
		while(1 ==0 && rightJoy.getRawButton(1) && gyro.getAngle() > 73.0){
			mainArmController.set(-1);
			secondaryArmController.set(rightJoy.getY(Joystick.Hand.kRight));
			Timer.delay(0.005);
		}		
	}
	mainArmController.set(0);
//MoveArm(.5, 1);
}

public void MiddleArmPosition()
{
	if(isDefaultArmPosition){DefaultArmPosition();}
	isDefaultArmPosition = false;
	dsLCD.println(DriverStationLCD.Line.kUser2,1,"Entering MiddleArmPosition");
MoveArm(1.3, 1.0);
}// end MiddleArmPosition()

public void TopArmPosition()
{
	if(isDefaultArmPosition){DefaultArmPosition();}
	isDefaultArmPosition = false;
	dsLCD.println(DriverStationLCD.Line.kUser2,1,"Entering TopArmPosition");
MoveArm(1.7, 1.05);
}// end TopArmPosition()
	
public void LoadArmPosition()
{
	if(isDefaultArmPosition){DefaultArmPosition();}
	isDefaultArmPosition = false;
	dsLCD.println(DriverStationLCD.Line.kUser2,1,"Entering LoadArmPosition");
MoveArm(.25, 2.25);
}// end LoadArmPosition()

public void FeedArmPosition()
{
	if(isDefaultArmPosition){DefaultArmPosition();}
	isDefaultArmPosition = false;
	dsLCD.println(DriverStationLCD.Line.kUser2,1,"Entering FeedArmPosition");
MoveArm(.5, 0.25);
}// end LoadArmPosition()


/**
this zeros out the location of the arms
*/
public void DefaultArmPosition()
{
	if(!gripState){	//if open close the gripper
	gripState = true;
	gripOpenSolenoid.set(false);
	Timer.delay(0.030);
	gripOpenSolenoid.set(true);
	}
	
	isDefaultArmPosition = true;
	//printf("Entering TravelPosition");
	int defaultCompleted=0;
	while(defaultCompleted != 1){
	myRobot.arcadeDrive(leftJoy); 
	if(!limitMainTravelPosition.get()){mainArmController.set(-.5);}else{mainArmController.set(0);}
	if(!limitSecondaryTravelPosition.get()){secondaryArmController.set(-.5);}else{secondaryArmController.set(0);}
	if(limitSecondaryTravelPosition.get() && limitMainTravelPosition.get()){defaultCompleted = 1;}	
	Timer.delay(0.005);
	}
	secondaryArmController.set(0);
	mainArmController.set(0);
	
}// end DefaultArmPosition()

/*
the function to move the arm(seconds)
main = main arm
secondary = secondary arm
*/
public void MoveArm(double Main, double Secondary)		//Main and Secondary are defined by seconds the arm motors need to run
{		
	DefaultArmPosition();
	//mainArm
	timer.reset();
	int moveFin = 0;
	
	while(moveFin != 1)
	{
	myRobot . arcadeDrive(leftJoy); 
				//speed for moving the arm
	if(timer.get() < Secondary){secondaryArmController.set(-.5);}else{secondaryArmController.set(0);}
	if(timer.get() < Main){mainArmController.set(1);}else{mainArmController.set(0);}
	dsLCD.println(DriverStationLCD.Line.kUser2,1,"timerMainArm(seconds): " + timer.get() + "\n");
	if(timer.get() > Main && timer.get() > Secondary){moveFin = 1;}
	Timer.delay(0.005);
	}
	mainArmController.set(0);
	secondaryArmController.set(0);
	
}// end LowArmPosition()

public void GripRelease()
{
	if(gripState){	//if open close the gripper
		gripState = !gripState;
		gripOpenSolenoid.set(true);
		Timer.delay(0.030);//0.030
		gripOpenSolenoid.set(false);
	} else {// open the gripper
		gripState = !gripState;
		gripCloseSolenoid.set(true);
		Timer.delay(0.030);
		gripCloseSolenoid.set(false);
	}
}// end GripRelease()

/*
 * minibot
 */
public void Deploy()
{
	/*
	while(!limitMinibot.get() && rightJoy . getRawButton(deploy) && rightJoy . getRawButton(trigger))
	{
		deployController.set(1);
		Timer.delay(0.005);
	}
	deployController.set(0);
	*/
}

/*
void OperatorControl(void)
	{
		while (IsOperatorControl())
		{
			while(leftJoy . getRawButton(low) && leftJoy . getRawButton(trigger))
				{
					mainArmController.set(0.5);			//speed for moving the arm
					Timer.delay(0.005);				// Timer.delay for a motor update time
				}
					mainArmController.set(0);	//reset the arm
					
			while(leftJoy . getRawButton(high) && leftJoy . getRawButton(trigger))
				{
					mainArmController.set(-1);			//speed for moving the arm
					Timer.delay(0.005);				// Timer.delay for a motor update time		
				}
					mainArmController.set(0);	//reset the ar m

			while(leftJoy . getRawButton(middle) && leftJoy . getRawButton(trigger))
				{
					secArmController.set(-0.4);			//speed for moving the arm
					Timer.delay(0.005);				// Timer.delay for a motor update time
				}
					secArmController.set(0);	//reset the arm
							
			while(leftJoy . getRawButton(load) && leftJoy . getRawButton(trigger))
				{
					secArmController.set(0.4);			//speed for moving the arm
					Timer.delay(0.005);				// Timer.delay for a motor update time		
				}
					secArmController.set(0);	//reset the arm
			
			mainArmController.set(leftJoy.getX(Joystick::Hand.kRight));			//speed for moving the arm
			if(leftJoy . getRawButton(high))
			{timer.Reset();
				while(timer.get() < 1.7)
				{
					mainArmController.set(-1);
					Timer.delay(0.005);
				}
					
				
				
					while(mainEnc.getVoltage() < 3)
						{
							mainArmController.set(-1);
							Timer.delay(0.005);
						}
					mainArmController.set(0);
					while(mainEnc.getVoltage() > 3)
						{
							mainArmController.set(-1);
							Timer.delay(0.005);
						}
					mainArmController.set(0);
					while(mainEnc.getVoltage() < 3)
						{
							mainArmController.set(-1);
							Timer.delay(0.005);
						}
				mainArmController.set(0);
				while(mainEnc.getVoltage() < 2)
					{
						mainArmController.set(-1);
						Timer.delay(0.005);
					}
			}//secArmController.set(leftJoy.getY(Joystick.Hand.kRight));			//speed for moving the arm

			Timer.delay(0.005);
		} // end operaterControl while
		
	}// end OperatorControl()*/
}





